/********************************************
* Copyrigt (C) rumi
* Module: drv_mpu9250.c
*********************************************/
#include "drv_mpu9250.h"

static const char *TAG = "drv_mpu9250.c";
/**************************Init Area*********************************/
//9255 dev1
PinConfig_DrvDef Pin_MPU9255_SCL = {
    .PinNum = GPIO_NUM_4,
};
PinConfig_DrvDef Pin_MPU9255_SDA = {
    .PinNum = GPIO_NUM_2,
};
iic_Node MPU9255_IIC = {
	.SCL = &Pin_MPU9255_SCL,
	.SDA = &Pin_MPU9255_SDA,
	.SpeedDelay = 28,
};
MPU9255_DataDef Dev_IMU9 = {
    .IIC = &MPU9255_IIC,
};

/*******************************************************************/
 
static uint8_t sGyroRawArray[14];

void Drv_MPU9255_Init(MPU9255_DataDef *device)
{
    uint8_t mpu_send_data;
    mpu_send_data = 0x00;
    Drv_IIC_WriteByte(device->IIC,MPU6500_Adress_7bit,MPU6500_PWR_MGMT_1,&mpu_send_data,1);
    mpu_send_data = 0x18;
    Drv_IIC_WriteByte(device->IIC,MPU6500_Adress_7bit,MPU6500_GYRO_CONFIG,&mpu_send_data,1);
    Drv_IIC_WriteByte(device->IIC,MPU6500_Adress_7bit,MPU6500_ACCEL_CONFIG,&mpu_send_data,1);
}

static void Calculate_Angles(float ax, float ay, float az, float *pitch, float *roll) {
    *pitch = atan2f(ay, sqrtf(ax * ax + az * az)) * RAD_TO_DEG;
    *roll = atan2f(-ax, sqrtf(ay * ay + az * az)) * RAD_TO_DEG;
    if (*pitch > 90.0f) *pitch -= 180.0f;
    else if (*pitch < -90.0f) *pitch += 180.0f;
    
    if (*roll > 90.0f) *roll -= 180.0f;
    else if (*roll < -90.0f) *roll += 180.0f;
}

void Calculate_Yaw(float mag_x, float mag_y, float mag_z, float pitch, float roll, float *yaw) {

    float pitch_rad = pitch * M_PI / 180.0f;
    float roll_rad = roll * M_PI / 180.0f;

    float mag_x_comp = mag_x * cosf(pitch_rad) + mag_z * sinf(pitch_rad);
    float mag_y_comp = mag_x * sinf(roll_rad) * sinf(pitch_rad) + 
                      mag_y * cosf(roll_rad) - 
                      mag_z * sinf(roll_rad) * cosf(pitch_rad);

    *yaw = atan2f(-mag_y_comp, mag_x_comp) * 180.0f / M_PI;
    if (*yaw < 0) *yaw += 360.0f;
}

void Drv_MPU9255_ReadEurla(MPU9255_DataDef *device)
{
    Drv_IIC_ReadByte(device->IIC,MPU6500_Adress_7bit,MPU6500_ACCEL_XOUT_H,sGyroRawArray,14);

    device->Gyro.Ace_X = (int16_t)(sGyroRawArray[0]<< 8)+sGyroRawArray[1];
    device->Gyro.Ace_Y = (int16_t)(sGyroRawArray[2]<< 8)+sGyroRawArray[3];
    device->Gyro.Ace_Z = (int16_t)(sGyroRawArray[4]<< 8)+sGyroRawArray[5];
    device->Gyro.Tmp   = (int16_t)(sGyroRawArray[6]<< 8)+sGyroRawArray[7];
    device->Gyro.Gyro_X = (int16_t)(sGyroRawArray[8]<< 8)+sGyroRawArray[9];
    device->Gyro.Gyro_Y = (int16_t)(sGyroRawArray[10]<< 8)+sGyroRawArray[11];
    device->Gyro.Gyro_Z = (int16_t)(sGyroRawArray[12]<< 8)+sGyroRawArray[13];

    device->Out.Ace_Xf = (float)device->Gyro.Ace_X/ACCEL_SENSITIVITY;
    device->Out.Ace_Yf = (float)device->Gyro.Ace_Y/ACCEL_SENSITIVITY;
    device->Out.Ace_Zf = (float)device->Gyro.Ace_Z/ACCEL_SENSITIVITY;
    device->Out.Tmpf   = (float)device->Gyro.Tmp/340.0f+36.53f;
    device->Out.Gyro_Xf = (float)device->Gyro.Gyro_X/GYRO_SENSITIVITY;
    device->Out.Gyro_Yf = (float)device->Gyro.Gyro_Y/GYRO_SENSITIVITY;
    device->Out.Gyro_Zf = (float)device->Gyro.Gyro_Z/GYRO_SENSITIVITY;

    Drv_IIC_ReadByte(device->IIC,AK8963_MAG_Adress_7BIT,MAG_XOUT_L,sGyroRawArray,6);
    device->Mag.Mag_X = (int16_t)(sGyroRawArray[0]<< 8)+sGyroRawArray[1];
    device->Mag.Mag_Y = (int16_t)(sGyroRawArray[2]<< 8)+sGyroRawArray[3];
    device->Mag.Mag_Z = (int16_t)(sGyroRawArray[4]<< 8)+sGyroRawArray[5];

    Calculate_Angles(device->Out.Ace_Xf, device->Out.Ace_Yf, device->Out.Ace_Zf, &device->Out.Pitch, &device->Out.Roll);
    Calculate_Yaw(device->Mag.Mag_X, device->Mag.Mag_Y, device->Mag.Mag_Z, device->Out.Pitch, device->Out.Roll, &device->Out.Yaw);

    //ESP_LOGI(TAG, "Eura X: %f, Y: %f, Z: %f", device->Out.Pitch, device->Out.Roll, device->Out.Yaw);

}

uint8_t Drv_MPU9255_Control(MPU9255_DataDef *device,uint8_t type)
{
    switch ((MPU9255_CTRL_EnumDef)type)
    {
    case E_MPU9250_INIT:
        Drv_MPU9255_Init(device);
        break;
    case E_MPU9250_READ:
        Drv_MPU9255_ReadEurla(device);
        break;
    default:
        break;
    }
    return 0;
}


/********************************************
 * End of file
*********************************************/
